#include "debug_task.hpp"
#if (IS_DEBUG_MODE == 0x01u)

#if (IS_DEBUG_MODE_FRICTION == 0x01u)
ECF_RC *debug_RC;
Fire_n::Firc_State_e *debug_fire_state;
//uint8_t *debug_gimbal_state;

float debug_kp;
float debug_ki;
float debug_kd;
float debug_i_max;
float debug_speed[3];
float debug_output[3];

#define DEBUG_NUM_AAA 0

namespace Debug_n
{
    Debug_c::Debug_c(void)
    {
        this->fire_ptr = Fire_n::Fire_c::Get_InstancePtr();
//        this->gimbal_ptr = Gimbal_n::Gimbal_c::Get_InstancePtr();
        debug_RC = ECF_RC::getInstance();
        debug_fire_state = &(this->fire_ptr->current_state);
//        debug_gimbal_state = this->gimbal_ptr->current_state;
        debug_kp = this->fire_ptr->friction_motor[DEBUG_NUM_AAA]->motor_controller.speed_PID.pid_measure_.Kp;
        debug_ki = this->fire_ptr->friction_motor[DEBUG_NUM_AAA]->motor_controller.speed_PID.pid_measure_.Ki;
        debug_kd = this->fire_ptr->friction_motor[DEBUG_NUM_AAA]->motor_controller.speed_PID.pid_measure_.Kd;
        debug_i_max = this->fire_ptr->friction_motor[DEBUG_NUM_AAA]->motor_controller.speed_PID.pid_measure_.max_Ierror;
    }

    void Debug_c::MainLoop()
    {
        this->fire_ptr->friction_motor[DEBUG_NUM_AAA]->motor_controller.speed_PID.pid_measure_.Kp = debug_kp;
        this->fire_ptr->friction_motor[DEBUG_NUM_AAA]->motor_controller.speed_PID.pid_measure_.Ki = debug_ki;
        this->fire_ptr->friction_motor[DEBUG_NUM_AAA]->motor_controller.speed_PID.pid_measure_.Kd = debug_kd;
        this->fire_ptr->friction_motor[DEBUG_NUM_AAA]->motor_controller.speed_PID.pid_measure_.max_Ierror = debug_i_max;
        debug_output[0] = this->fire_ptr->friction_motor[0]->motor_controller.speed_PID.pid_measure_.out;
        debug_output[1] = this->fire_ptr->friction_motor[1]->motor_controller.speed_PID.pid_measure_.out;
        debug_output[2] = this->fire_ptr->friction_motor[2]->motor_controller.speed_PID.pid_measure_.out;
        debug_speed[0] = this->fire_ptr->friction_motor[0]->MotorMeasure.measure.feedback_speed;
        debug_speed[1] = this->fire_ptr->friction_motor[1]->MotorMeasure.measure.feedback_speed;
        debug_speed[2] = this->fire_ptr->friction_motor[2]->MotorMeasure.measure.feedback_speed;
    }

    void Debug_c::MotorLoop()
    {
        DJI_Motor_n::DJIMotorControl();
    }

    void Debug_c::StateLoop()
    {
        Fire_n::StateLoop();
//        Gimbal_n::StateLoop();
        CMD_n::StateLoop();
    }
}
#endif // IS_DEBUG_MODE_FRICTION

#if (IS_DEBUG_MODE_GIMBAL == 0x01u)
ECF_RC *debug_RC;
Gimbal_n::Gimbal_State_e *debug_gimbal;

float debug_yaw_motor_feedback;
float debug_pitch_motor_feedback;
float debug_yaw_set;
float debug_pitch_set;

namespace Debug_n
{
    Debug_c::Debug_c(void)
    {
        this->gimbal_ptr = Gimbal_n::Gimbal_c::Get_InstancePtr();
        debug_RC = ECF_RC::getInstance();
        debug_gimbal = &(this->gimbal_ptr->current_state);
    }

    void Debug_c::MainLoop()
    {
        CMD_n::StateLoop();
        debug_yaw_motor_feedback = (this->gimbal_ptr->yaw->motor->MotorMeasure.measure.record_ecd);
        debug_pitch_motor_feedback = (this->gimbal_ptr->pitch->motor->get_data_.postion);
        debug_yaw_set = (this->gimbal_ptr->target_yaw);
        debug_pitch_set = (this->gimbal_ptr->target_pitch);
    }

    void Debug_c::MotorLoop()
    {
        DJI_Motor_n::DJIMotorControl();
        this->gimbal_ptr->pitch->motor->Transmit(0,0,0,0,0);
    }

    void Debug_c::StateLoop()
    {
//        Fire_n::StateLoop();
        Gimbal_n::StateLoop();
    }
}
#endif // IS_DEBUG_MODE_GIMBAL
#endif // IS_DEBUG_MODE
